/*
This script is designed to upload COAXIAL thrust data 
to the RCbenchmark database. It is compatible only
with the Series 1780 because at this time the Series 1780
is the only product supporting two motors simultaneously.

When testing coaxial, there is interaction between the
two propellers due to the shared airflow. Therfore the data
is only meaningful when both sides are recorded together.

You must specify what throttle pairs to test for. The
script will automatically capture data for each throttle
pair specified. For example:

1200, 1400
1250, 1400
1300, 1400

This would take three sample points. At each point, the
throttle of side A varies, while side B is kept constant.

You should design the test sequence to cover as much as
possible in terms of the motor and propeller rotations
speeds. Because the number of data points can increase
very quickly (testing 10 throttle points of side A along
with 10 data points of side B would mean 100 test points).
You should focus on the important points. At high throttle
your power source may discharge very quickly. In this case,
it is recommended to post your data in multiple tests. In
the future, the database will be able to merge the data from
multiple tests for analysis.
*/

////////// Script parameters ///////// */

// List of coaxial test points: A side (µs), B side (µs)
var throttlePoints = @@{
  "type": "csv",
  "name": "Throttle points (µs): A, B",
  "hint": "Define the list of throttle pairs for each test point. Each line should have two values, separated by a comma. The first column represents the throttle on side A, and the second column represents the side B throttle.",
  "default": "1200, 1400\n1250, 1400\n1300, 1400\n1200, 1450"
}##;

var escAStart = @@{
  "type": "integer",
  "min": 700,
  "max": 1600,
  "name": "ESCA init value (µs)",
  "default": 1000
}##; // ESC idle value
var escBStart = @@{
  "type": "integer",
  "min": 700,
  "max": 1600,
  "name": "ESCB init value (µs)",
  "default": 1000
}##; // ESC idle value
var escInitTime = @@{
  "type": "float",
  "min": 1,
  "max": 150,
  "name": "ESC init time (s)",
  "hint": "How long to keep the ESC init value before starting the test. Usually 4 seconds is sufficient, but a longer time may be useful to delay the start of a test.",
  "default": 4
}##; // Hold time at ESC idle

var settlingTime_s = @@{
  "type": "float",
  "min": 1,
  "max": 20,
  "name": "Settling time (s)",
  "hint": "Ensure the settling time is sufficiently long to let the sensors stabilize after a change of throttle.",
  "default": 3
}##; // Settling time before measurement
    
var cooldownTime_s = @@{
  "type": "float",
  "min": 0,
  "max": 1200,
  "name": "Cooling time each step (s)",
  "hint": "Use this setting if the components under test cannot withstand a continuous throttle without overheating.",
  "default": 0
}##; // If the motor needs to cool down between steps
    
var cooldownThrottleA_us = @@{
  "type": "integer",
  "min": 700,
  "max": 1600,
  "name": "Cooling throttle A (µs)",
  "hint": "A slowly spining motor may actually cool down faster than a stopped motor.",
  "default": 1000
}##; // Cool down faster when slowly spinning

var cooldownThrottleB_us = @@{
  "type": "integer",
  "min": 700,
  "max": 1600,
  "name": "Cooling throttle B (µs)",
  "hint": "A slowly spining motor may actually cool down faster than a stopped motor.",
  "default": 1000
}##; // Cool down faster when slowly spinning
    
var max_slew_rate_us_per_s = @@{
  "type": "integer",
  "min": 0,
  "max": 10000,
  "name": "Max. slew rate (µs/s)",
  "hint": "This sets how fast the throttle signal is allowed to change. It helps avoid high torque when transitioning between two steps. Also helps prevent damage to the power supply by limiting braking speed. Set to zero to disable.",
  "default": 50
}##; // Limits torque from throttle changes, but adds extra test time

var samplesAvg = @@{
  "type": "integer",
  "min": 10,
  "max": 5000,
  "name": "Samples to average",
  "hint": "Minimum is 10, but it can be set higher for higher quality results, especially if there are a lot of vibrations.",
  "default": 25
}##;     // Number of samples to average

var save_CSV_file = @@{
  "type": "boolean",
  "name": "Save data to CSV file?",
  "hint": "For your own records, you may optionally save the data to a CSV file, in addition to uploading to the database.",
  "default": false
}##; // Save data to CSV file
   

//////// Beginning of the script /////////
if(rcb.getBoardVersion() !== "Series 1780"){
  rcb.console.error("Error: this script can only be used with the Series 1780.");
}

if(!rcb.vars.s1780detected || !rcb.vars.s1780detected.Full){
  rcb.console.error("Error: ensure you connected the load cell and power sensor for both side A and side B.");
}

rcb.console.print('<strong>Checklist:</strong><br/><br/> 1) Are the torque, thrust, and current values positive for each side? Check by spinning the motors from the "Manual Control" tab first, and if needed invert the sign from the "Utilities" tab.<br/><br/> 2) Are the torque and thrust calibrations verified for each side?<br/><br/> 3) Did you test the system at the specified throttle values manually before running this test?<br/><br/>Please type "<strong>Y</strong>" on your keyboard to confirm.');
rcb.onKeyboardPress(function(key){
  if(key === 89){ //Y
    initESC(function(){
      tare(function(){
        takeSample(step);
      });
    });
  }else{
    rcb.endScript();
  }
});

var outputs = ["escA","escB"];
var firstRowAdded = false;
var stepState = 0;
var lastThrottle = [escAStart, escBStart];

// validate the throttle points
throttlePoints.forEach(function(line){
  if(line.length !== 2){
    rcb.console.error("Error: throttle points should come in pairs of two.");
  }
  line.forEach(function(el){
    if(isNaN(el) || !Number.isInteger(el)){
      rcb.console.error("Error: throttle points should be integers. Given: " + el);
    }
    if(el < 700 || el > 2300){
      rcb.console.error("Error: throttle points should be between 700 and 2300 µs.");
    }
  });
});

//Starting a new CSV log file
if(save_CSV_file){
  rcb.files.newLogFile({prefix: @@@FILENAME###});
}

//Tare the load cells and the current sensor
function tare(callback){
  rcb.sensors.tareLoadCells(function(){
    //taring current has an effect only in the Series 1780
    rcb.sensors.tareCurrent(callback);
  });
}

//Arms the ESC
function initESC(callback){
    //ESC initialization
    rcb.console.print("Initializing ESC...");
    rcb.database.log("ESC A/B init values (µs): [" + escAStart + ", " + escBStart + "]");
    rcb.output.set(outputs, [escAStart, escBStart]);
    rcb.wait(callback, escInitTime);
}

//Performs steps, until no more.
function step(){
  switch(stepState) {
      
    // ramp up to next data point
    case 0: 
      if(throttlePoints.length > 0){
        var nextPoint = throttlePoints.shift();
        rcb.console.print("Testing at : [" + nextPoint[0] + ", " + nextPoint[1] + "]");
        var rampUpTime_s = calcRampTime(lastThrottle, nextPoint);
        rcb.output.ramp(outputs, lastThrottle, nextPoint, rampUpTime_s, step);
        lastThrottle = nextPoint;
        stepState++;
      }else{
        endFct(); // no more points to test, finish the script
      }
      break;
      
    // stabilization time then take a measurement.
    case 1: 
      rcb.wait(function(){
        takeSample(step);
      }, settlingTime_s);
      
      // next step will depend on if there is cooldown time specified
      if(cooldownTime_s > 0){
        stepState++;
      }else{
        stepState = 0;
      }
      break;
        
    // ramp down to specified cooling throttle
    case 2: 
      var coolDownThrottle = [cooldownThrottleA_us, cooldownThrottleB_us];
      var rampDownTime_s = calcRampTime(lastThrottle, coolDownThrottle);
      rcb.output.ramp(outputs, lastThrottle, coolDownThrottle, rampDownTime_s, step);
      lastThrottle = coolDownThrottle;
      stepState++;
      break;
                      
    // wait for cooldown time, then go to the next step
    case 3: 
      rcb.console.print("Cooling down...");                
      rcb.wait(step, cooldownTime_s);
      stepState = 0;
      break;
  }
}
  
// calculates the time needed to perform a ramp from and to
// without exceeding the max slew rate parameter.
function calcRampTime(from, to){
  var diffA_us = Math.abs(from[0]-to[0]);
  var diffB_us = Math.abs(from[1]-to[1]);
  var diff_us = Math.max(diffA_us, diffB_us);
  if(max_slew_rate_us_per_s > 0){
    rcb.console.print("Ramping...");
    return diff_us / max_slew_rate_us_per_s;
  }
  return 0;
}
  
// Records a sample to database
function takeSample(callback){
    rcb.sensors.read(function (result){
        var rpmA = result.motorOpticalSpeed.workingValue;
        var rpmB = result.motorOpticalSpeed.workingValue;
        var thrustA = result.thrust.workingValue;
        var thrustB = result.thrustB.workingValue;
        var torqueA = result.torque.workingValue;
        var torqueB = result.torqueB.workingValue;
        var currentA = result.current.workingValue;
        var currentB = result.currentB.workingValue;
        var maxThrust = math.max(thrustA, thrustB);
        var maxTorque = math.max(torqueA, torqueB);
        var maxCurrent = math.min(currentA, currentB);
        var minThrust = math.min(thrustA, thrustB);
        var minTorque = math.min(torqueA, torqueB);
        var minCurrent = math.min(currentA, currentB);
      
        // validate torque, thrust, and current are positive
        // we should accept some negative values for the slowest spinning side, as airflow can actually drive the other prop in reverse (assuming realistically will never exceeed 20%).
        if(maxThrust < -0.1 || minThrust + 0.1 < -0.2 * maxThrust){
          rcb.console.error("The database expects positive thrust values. Please reverse the thrust sign from the utilities tab.");
        }
        if(maxTorque < -0.1 || minTorque + 0.1 < -0.2 * maxTorque){
          rcb.console.error("The database expects positive torque values. Please reverse the torque sign from the utilities tab.");
        }
        if(maxCurrent < -1.0 || minCurrent + 1.0 < -0.2 * maxCurrent){
          rcb.console.error("Negative current measured. The database only supports positive current values.");
        }
        
        // do not add more than one rpm=0 rows
        if(!firstRowAdded && (rpmA > 0 || rpmB > 0)){
          rcb.console.error("The first row of the data should be at zero rpm.");
        }
        if(!firstRowAdded || rpmA > 0 || rpmB > 0){
          rcb.database.addData(result);
          firstRowAdded = true;
        } 
      
        if(save_CSV_file){
          rcb.files.newLogEntry(result, callback);
        }else{
          callback();
        }
    }, samplesAvg);
}

// Called at the end of the steps
function endFct(){
    // ramp down the motor back to initial throttle
    var finishThrottle = [escAStart, escBStart];
    var rampTime_s = calcRampTime(lastThrottle, finishThrottle);
    rcb.output.ramp(outputs, lastThrottle, finishThrottle, rampTime_s, function(){
      // give time for the motor to reach a full stop
      rcb.wait(function(){
        const isCoaxial = true;
        rcb.database.submit(rcb.endScript, isCoaxial);
      }, 1);
    });
}
